#include<../data_tools/usefull_routines.c>

int main(int argc, char *argv[])
{

  int i,j,k,nsnapshots,ncore;
  int ngas,ndm,ndisk,nsmmp,ntmmp,nfmmp;
  int *ind,*ind0,*ind1,*ind2,*ind3,*ind4,*ind5;
  double rcmSMMP[3],vcmSMMP[3],lSMMP[3];
  double rcmTMMP[3],vcmTMMP[3],lTMMP[3];
  double rcmFMMP[3],vcmFMMP[3],lFMMP[3];
  double mass,time,r,v,l,v2;
  char *inputfile,name_snapshots[100];
  CM cmMMP;

  FILE *posSMMP,*posTMMP,*posFMMP;
  posSMMP = fopen("position_SMMP.dat","w");
  posTMMP = fopen("position_TMMP.dat","w");
  posFMMP = fopen("position_FMMP.dat","w");

  inputfile=argv[1];
  nsnapshots = atoi(argv[2]);
  ncore = atoi(argv[3]);

  printf("\n");
  printf("#############################################################\n");
  printf("Obtening Rcm and Lcm for satellites for simulation:\n");
  printf("%s\n",argv[1]);
  printf("#############################################################\n");
  printf("\n");
 
  for(i=0; i<=nsnapshots; i++)
    { 

      if(i!=0);
      {
	particulas *particles;
	particles = (particulas *)malloc((size_t)N_tot*sizeof(particulas));
      }

      sprintf(name_snapshots,"%s_%.3d",inputfile,i);

      printf("\nSnapshot : %s\n",name_snapshots);
              
      //Load data file
      readGadgetBinary("option_makefile.dat",name_snapshots);

      ngas = Header.npartTotal[0];
      ndm = Header.npartTotal[1];
      ndisk = Header.npartTotal[2];
      nsmmp = Header.npartTotal[3];
      ntmmp = Header.npartTotal[4];
      nfmmp = Header.npartTotal[5];
      
      ind = malloc(N_tot*sizeof(int));
      if(ind == NULL){
	printf("Allocation error routines:81\n");
	exit(0);
      }
      
      for(k=0; k<N_tot; k++) 
	ind[k] = particles[k].id;
  
      ind0 = malloc(ngas*sizeof(int));
      if(ind0 == NULL){
	printf("Allocation error routines:81\n");
	exit(0);
      }
      ind1 = malloc(ndm*sizeof(int));
      if(ind1 == NULL){
	printf("Allocation error routines:81\n");
	exit(0);
      }
      
      ind2 = malloc(ndisk*sizeof(int));
      if(ind2 == NULL){
	printf("Allocation error routines:81\n");
	exit(0);
      }
      
      //printf("ndm %d nb %d\n",ndm,ndisk);
      
      for(k=(ngas); k<(ngas+ndm); k++) 
	{
	  ind1[k-ngas] =  particles[k].id;
	}
      //printf("ind2 %d i %d\n",ind2[i-1],i);
      
      printf("Computing center of mass for dark matter\n");
      centerMass(ind1, ndm, 0, 0, &cmMMP);
      compute_angmom(ind1, ndm, 0, 0, &cmMMP);
      printf("RCM : %e %e %e\n",cmMMP.cm[0],cmMMP.cm[1],cmMMP.cm[2]);
      printf("VCM : %e %e %e\n",cmMMP.vcm[0],cmMMP.vcm[1],cmMMP.vcm[2]);
      printf("LCM : %e %e %e\n",cmMMP.lcm[0],cmMMP.lcm[1],cmMMP.lcm[2]);
      printf("Done.\n");
      printf("\n");
      
      //Traslating galaxy to center of mass of the dark matter
      printf("Traslating galaxy to center of mass of the dark matter\n");
      traslate(&cmMMP, ind, N_tot);
      centerMass(ind1, ndm, 0, 0, &cmdummy);
      compute_angmom(ind1, ndm, 0, 0, &cmdummy);
      
      printf("NEW RCM : %e %e %e\n",cmdummy.cm[0],cmdummy.cm[1],cmdummy.cm[2]);
      printf("NEW VCM : %e %e %e\n",cmdummy.vcm[0],cmdummy.vcm[1],cmdummy.vcm[2]);
      printf("NEW LCM : %e %e %e\n",cmdummy.lcm[0],cmdummy.lcm[1],cmdummy.lcm[2]); 
      printf("Done.\n");
      
      //Orientate the galaxy with the angular momentum of the dark matter
      printf("\n Orientate galaxy with the angular momentum of the dark matter\n");
      printf("using %f %f %f\n", cmdummy.lcm[0], cmdummy.lcm[1], cmdummy.lcm[2]);
      float posL[3];
      posL[0] = cmdummy.lcm[0];
      posL[1] = cmdummy.lcm[1];
      posL[2] = cmdummy.lcm[2];
      rotate(posL, ind, N_tot);
      centerMass(ind1, ndm, 0, 0, &cmdummy);
      compute_angmom(ind1, ndm, 0, 0, &cmdummy);
      
      printf("NEW RCM : %e %e %e\n",cmdummy.cm[0],cmdummy.cm[1],cmdummy.cm[2]);
      printf("NEW VCM : %e %e %e\n",cmdummy.vcm[0],cmdummy.vcm[1],cmdummy.vcm[2]);
      printf("NEW LCM : %e %e %e\n",cmdummy.lcm[0],cmdummy.lcm[1],cmdummy.lcm[2]); 
      printf("Done.\n");
      printf("\n");

	       
      if(i==0)
	{

	  //#####################################
	  //             ALL
	  //####################################
	  double *E,*Ep,*Ek;
	  FILE *energy;
	  energy = fopen("energy.dat","w");
	  //size_t *IndSMMP;
	  
	  E = (double *)malloc((size_t)N_tot*sizeof(double));
	  Ek = (double *)malloc((size_t)N_tot*sizeof(double));
	  Ep = (double *)malloc((size_t)N_tot*sizeof(double));
	  // IndSMMP = (size_t *)malloc((size_t)nsmmp*sizeof(size_t));

	  for(k=0; k<N_tot; k++)
	    {
	      v2 = particles[k].vel[0]*particles[k].vel[0]
		+particles[k].vel[1]*particles[k].vel[1]
		+particles[k].vel[2]*particles[k].vel[2];

	      Ek[k] = 0.5*particles[k].mass*v2;
	      Ep[k] = particles[k].pot*particles[k].mass;  
	      E[k] = Ek[k] + Ep[k];
	      // IndSMMP[k-ngas-ndm-ndisk] =  particles[k].id;
	      //  IndSMMP[k-ngas-ndm-ndisk] =  k;
	      if(k>=ndm)
		{
		  fprintf(energy,"%e %e %e %e %e %e\n",
			  particles[k].pos[0],particles[k].pos[1],particles[k].pos[2],
			  E[k],Ek[k],Ep[k]); 
		}
	    }
	  fclose(energy);

	  //#####################################
	  //             SMMP
	  //####################################

	  ind3 = (int *)malloc(ncore*sizeof(int));
	  if(ind3 == NULL){
	    printf("Allocation error routines:81\n");
	    exit(0);
	  }
     
	  double *ESMMP,*EpSMMP,*EkSMMP;
	  size_t *IndSMMP,*pSMMP;

	  ESMMP = (double *)malloc((size_t)nsmmp*sizeof(double));
	  EpSMMP = (double *)malloc((size_t)nsmmp*sizeof(double));
	  EkSMMP = (double *)malloc((size_t)nsmmp*sizeof(double));
	  IndSMMP = (size_t *)malloc((size_t)nsmmp*sizeof(size_t));
	  pSMMP = (size_t *)malloc((size_t)nsmmp*sizeof(size_t));

	  for(k=(ngas+ndm+ndisk); k<(ngas+ndm+ndisk+nsmmp); k++)
	    {
	      v2 = particles[k].vel[0]*particles[k].vel[0]
		+particles[k].vel[1]*particles[k].vel[1]
		+particles[k].vel[2]*particles[k].vel[2];

	      EkSMMP[k-ngas-ndm-ndisk] = 0.5*particles[k].mass*v2;
	      EpSMMP[k-ngas-ndm-ndisk] = particles[k].pot*particles[k].mass;  
	      ESMMP[k-ngas-ndm-ndisk] = EkSMMP[k-ngas-ndm-ndisk] + EpSMMP[k-ngas-ndm-ndisk];
	      IndSMMP[k-ngas-ndm-ndisk] =  k;
	    }
	  
	  gsl_sort_index(pSMMP,ESMMP,1,nsmmp);

	  FILE *bindingSMMP;
	  bindingSMMP = fopen("ligadas_SMMP.dat","w");

	  for(j=0; j<ncore; j++)
	    {
	      // ind3[j] = particles[IndSMMP[j]].id;
	      ind3[j] = IndSMMP[pSMMP[j]];
	      //printf("%d %d %d %e\n",pSMMP[j],IndSMMP[pSMMP[j]],ind3[j],ESMMP[pSMMP[j]]); 
	      fprintf(bindingSMMP,"%e %e %e %e %e %e\n",
		      particles[ind3[j]].pos[0],particles[ind3[j]].pos[1],particles[ind3[j]].pos[2],
		      ESMMP[pSMMP[j]],EkSMMP[pSMMP[j]],EpSMMP[pSMMP[j]]); 
	      
	    }

	  fclose(bindingSMMP);
	  free(pSMMP);
	  free(ESMMP);
	  free(EpSMMP);
	  free(EkSMMP);
	  free(IndSMMP);

	  //#####################################
	  //             TMMP
	  //####################################


	  ind4 = (int *)malloc(ncore*sizeof(int));
	  if(ind4 == NULL){
	    printf("Allocation error routines:81\n");
	    exit(0);
	  }
     

	  double *ETMMP,*EpTMMP,*EkTMMP;
	  size_t *IndTMMP,*pTMMP;

	  ETMMP = (double *)malloc((size_t)ntmmp*sizeof(double));
	  EpTMMP = (double *)malloc((size_t)ntmmp*sizeof(double));
	  EkTMMP = (double *)malloc((size_t)ntmmp*sizeof(double));
	  IndTMMP = (size_t *)malloc((size_t)ntmmp*sizeof(size_t));
	  pTMMP = (size_t *)malloc((size_t)ntmmp*sizeof(size_t));

	  for(k=(ngas+ndm+ndisk+nsmmp); k<(ngas+ndm+ndisk+nsmmp+ntmmp); k++)
	    {
	      v2 = particles[k].vel[0]*particles[k].vel[0]
		+particles[k].vel[1]*particles[k].vel[1]
		+particles[k].vel[2]*particles[k].vel[2];

	      EkTMMP[k-ngas-ndm-ndisk-nsmmp] = 0.5*particles[k].mass*v2;
	      EpTMMP[k-ngas-ndm-ndisk-nsmmp] = particles[k].pot*particles[k].mass;  
	      ETMMP[k-ngas-ndm-ndisk-nsmmp] = EkTMMP[k-ngas-ndm-ndisk-nsmmp] + EpTMMP[k-ngas-ndm-ndisk-nsmmp];
	      IndTMMP[k-ngas-ndm-ndisk-nsmmp] =  k;
	    }
	  
	  gsl_sort_index(pTMMP,ETMMP,1,ntmmp);

	  FILE *bindingTMMP;
	  bindingTMMP = fopen("ligadas_TMMP.dat","w");

	  for(j=0; j<ncore; j++)
	    {
	      // ind3[j] = particles[IndTMMP[j]].id;
	      ind4[j] = IndTMMP[pTMMP[j]];
	      //printf("%d %d %d %e\n",pTMMP[j],IndTMMP[pTMMP[j]],ind4[j],ETMMP[pTMMP[j]]); 
	      fprintf(bindingTMMP,"%e %e %e %e %e %e\n",
		      particles[ind4[j]].pos[0],particles[ind4[j]].pos[1],particles[ind4[j]].pos[2],
		      ETMMP[pTMMP[j]],EkTMMP[pTMMP[j]],EpTMMP[pTMMP[j]]); 
	      
	    }

	  fclose(bindingTMMP);
	  free(pTMMP);
	  free(ETMMP);
	  free(EpTMMP);
	  free(EkTMMP);
	  free(IndTMMP);

  
	  //#####################################
	  //             FMMP
	  //####################################

	  ind5 = (int *)malloc(ncore*sizeof(int));
	  if(ind5 == NULL){
	    printf("Allocation error routines:81\n");
	    exit(0);
	  }
     

	  double *EFMMP,*EpFMMP,*EkFMMP;
	  size_t *IndFMMP,*pFMMP;

	  EFMMP = (double *)malloc((size_t)nfmmp*sizeof(double));
	  EpFMMP = (double *)malloc((size_t)nfmmp*sizeof(double));
	  EkFMMP = (double *)malloc((size_t)nfmmp*sizeof(double));
	  IndFMMP = (size_t *)malloc((size_t)nfmmp*sizeof(size_t));
	  pFMMP = (size_t *)malloc((size_t)nfmmp*sizeof(size_t));

	  for(k=(ngas+ndm+ndisk+nsmmp+ntmmp); k<(ngas+ndm+ndisk+nsmmp+ntmmp+nfmmp); k++)
	    {
	      v2 = particles[k].vel[0]*particles[k].vel[0]
		+particles[k].vel[1]*particles[k].vel[1]
		+particles[k].vel[2]*particles[k].vel[2];

	      EkFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp] = 0.5*particles[k].mass*v2;
	      EpFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp] = particles[k].pot*particles[k].mass;  
	      EFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp] = EkFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp] + EpFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp];
	      IndFMMP[k-ngas-ndm-ndisk-nsmmp-ntmmp] =  k;
	    }
	  
	  gsl_sort_index(pFMMP,EFMMP,1,nfmmp);

	  FILE *bindingFMMP;
	  bindingFMMP = fopen("ligadas_FMMP.dat","w");

	  for(j=0; j<ncore; j++)
	    {
	      // ind3[j] = particles[IndFMMP[j]].id;
	      ind5[j] = IndFMMP[pFMMP[j]];
	      //printf("%d %d %d %e\n",pFMMP[j],IndFMMP[pFMMP[j]],ind5[j],EFMMP[pFMMP[j]]); 
	      fprintf(bindingFMMP,"%e %e %e %e %e %e\n",
		      particles[ind5[j]].pos[0],particles[ind5[j]].pos[1],particles[ind5[j]].pos[2],
		      EFMMP[pFMMP[j]],EkFMMP[pFMMP[j]],EpFMMP[pFMMP[j]]); 
	      
	    }

	  fclose(bindingFMMP);
	  free(pFMMP);
	  free(EFMMP);
	  free(EpFMMP);
	  free(EkFMMP);
	  free(IndFMMP);
  	  
	}
      

      //#####################################
      //             SMMP
      //####################################
      

      rcmSMMP[0]=rcmSMMP[1]=rcmSMMP[2]=0.0;
      vcmSMMP[0]=vcmSMMP[1]=vcmSMMP[2]=0.0;
      lSMMP[0]=lSMMP[1]=lSMMP[2]=0.0;
      mass = 0.0;
      
      for(j=0; j<ncore; j++)
	{
	  //	  printf("%d %d\n",j,ind3[j]);
	  
	  rcmSMMP[0] = rcmSMMP[0] + particles[ind3[j]].pos[0]*particles[ind3[j]].mass;
	  rcmSMMP[1] = rcmSMMP[1] + particles[ind3[j]].pos[1]*particles[ind3[j]].mass;
	  rcmSMMP[2] = rcmSMMP[2] + particles[ind3[j]].pos[2]*particles[ind3[j]].mass;
	  
	  vcmSMMP[0] = vcmSMMP[0] + particles[ind3[j]].vel[0]*particles[ind3[j]].mass;
	  vcmSMMP[1] = vcmSMMP[1] + particles[ind3[j]].vel[1]*particles[ind3[j]].mass;
	  vcmSMMP[2] = vcmSMMP[2] + particles[ind3[j]].vel[2]*particles[ind3[j]].mass;
	  
	 
	  mass = mass + particles[ind3[j]].mass;	  
	}
      
      rcmSMMP[0] = rcmSMMP[0]/mass;
      rcmSMMP[1] = rcmSMMP[1]/mass;
      rcmSMMP[2] = rcmSMMP[2]/mass;
      
      vcmSMMP[0] = vcmSMMP[0]/mass;
      vcmSMMP[1] = vcmSMMP[1]/mass;
      vcmSMMP[2] = vcmSMMP[2]/mass;
      
      lSMMP[0] = (rcmSMMP[1]*vcmSMMP[2] - rcmSMMP[2]*vcmSMMP[1]);
      lSMMP[1] = - (rcmSMMP[0]*vcmSMMP[2] - rcmSMMP[2]*vcmSMMP[0]);
      lSMMP[2] = (rcmSMMP[0]*vcmSMMP[1] - rcmSMMP[1]*vcmSMMP[0]);

      time = i*5.0/200;
      r = sqrt(rcmSMMP[0]*rcmSMMP[0]+rcmSMMP[1]*rcmSMMP[1]+rcmSMMP[2]*rcmSMMP[2]);
      v = sqrt(vcmSMMP[0]*vcmSMMP[0]+vcmSMMP[1]*vcmSMMP[1]+vcmSMMP[2]*vcmSMMP[2]);
      l = sqrt(lSMMP[0]*lSMMP[0]+lSMMP[1]*lSMMP[1]+lSMMP[2]*lSMMP[2]);

      fprintf(posSMMP,"%lf %e %e %e %e %e %e %e %e %e %e %e %e\n",
	      time,
	      rcmSMMP[0],rcmSMMP[1],rcmSMMP[2],r,
	      vcmSMMP[0],vcmSMMP[1],vcmSMMP[2],v,
	      lSMMP[0],lSMMP[1],lSMMP[2],l); 
     
      //#####################################
      //             TMMP
      //####################################
      

      rcmTMMP[0]=rcmTMMP[1]=rcmTMMP[2]=0.0;
      vcmTMMP[0]=vcmTMMP[1]=vcmTMMP[2]=0.0;
      lTMMP[0]=lTMMP[1]=lTMMP[2]=0.0;
      mass = 0.0;
      
      for(j=0; j<ncore; j++)
	{
	  //	  printf("%d %d\n",j,ind4[j]);
	  rcmTMMP[0] = rcmTMMP[0] + particles[ind4[j]].pos[0]*particles[ind4[j]].mass;
	  rcmTMMP[1] = rcmTMMP[1] + particles[ind4[j]].pos[1]*particles[ind4[j]].mass;
	  rcmTMMP[2] = rcmTMMP[2] + particles[ind4[j]].pos[2]*particles[ind4[j]].mass;
	  
	  vcmTMMP[0] = vcmTMMP[0] + particles[ind4[j]].vel[0]*particles[ind4[j]].mass;
	  vcmTMMP[1] = vcmTMMP[1] + particles[ind4[j]].vel[1]*particles[ind4[j]].mass;
	  vcmTMMP[2] = vcmTMMP[2] + particles[ind4[j]].vel[2]*particles[ind4[j]].mass;

	  mass = mass + particles[ind4[j]].mass;	  
	}

      rcmTMMP[0] = rcmTMMP[0]/mass;
      rcmTMMP[1] = rcmTMMP[1]/mass;
      rcmTMMP[2] = rcmTMMP[2]/mass;
      
      vcmTMMP[0] = vcmTMMP[0]/mass;
      vcmTMMP[1] = vcmTMMP[1]/mass;
      vcmTMMP[2] = vcmTMMP[2]/mass;
      
      lTMMP[0] = (rcmTMMP[1]*vcmTMMP[2] - rcmTMMP[2]*vcmTMMP[1]);
      lTMMP[1] = -(rcmTMMP[0]*vcmTMMP[2] - rcmTMMP[2]*vcmTMMP[0]);
      lTMMP[2] = (rcmTMMP[0]*vcmTMMP[1] - rcmTMMP[1]*vcmTMMP[0]);

      time = i*5.0/200;
      r = sqrt(rcmTMMP[0]*rcmTMMP[0]+rcmTMMP[1]*rcmTMMP[1]+rcmTMMP[2]*rcmTMMP[2]);
      v = sqrt(vcmTMMP[0]*vcmTMMP[0]+vcmTMMP[1]*vcmTMMP[1]+vcmTMMP[2]*vcmTMMP[2]);
      l = sqrt(lTMMP[0]*lTMMP[0]+lTMMP[1]*lTMMP[1]+lTMMP[2]*lTMMP[2]);

      fprintf(posTMMP,"%lf %e %e %e %e %e %e %e %e %e %e %e %e\n",
	      time,
	      rcmTMMP[0],rcmTMMP[1],rcmTMMP[2],r,
	      vcmTMMP[0],vcmTMMP[1],vcmTMMP[2],v,
	      lTMMP[0],lTMMP[1],lTMMP[2],l); 


      //#####################################
      //             FMMP
      //####################################
      

      rcmFMMP[0]=rcmFMMP[1]=rcmFMMP[2]=0.0;
      vcmFMMP[0]=vcmFMMP[1]=vcmFMMP[2]=0.0;
      lFMMP[0]=lFMMP[1]=lFMMP[2]=0.0;
      mass = 0.0;
      
      for(j=0; j<ncore; j++)
	{
	  //	  printf("%d %d\n",j,ind5[j]);
	  rcmFMMP[0] = rcmFMMP[0] + particles[ind5[j]].pos[0]*particles[ind5[j]].mass;
	  rcmFMMP[1] = rcmFMMP[1] + particles[ind5[j]].pos[1]*particles[ind5[j]].mass;
	  rcmFMMP[2] = rcmFMMP[2] + particles[ind5[j]].pos[2]*particles[ind5[j]].mass;
	  
	  vcmFMMP[0] = vcmFMMP[0] + particles[ind5[j]].vel[0]*particles[ind5[j]].mass;
	  vcmFMMP[1] = vcmFMMP[1] + particles[ind5[j]].vel[1]*particles[ind5[j]].mass;
	  vcmFMMP[2] = vcmFMMP[2] + particles[ind5[j]].vel[2]*particles[ind5[j]].mass;

	  mass = mass + particles[ind5[j]].mass;	  
	}

      rcmFMMP[0] = rcmFMMP[0]/mass;
      rcmFMMP[1] = rcmFMMP[1]/mass;
      rcmFMMP[2] = rcmFMMP[2]/mass;
      
      vcmFMMP[0] = vcmFMMP[0]/mass;
      vcmFMMP[1] = vcmFMMP[1]/mass;
      vcmFMMP[2] = vcmFMMP[2]/mass;
      
      lFMMP[0] = (rcmFMMP[1]*vcmFMMP[2] - rcmFMMP[2]*vcmFMMP[1]);
      lFMMP[1] = - (rcmFMMP[0]*vcmFMMP[2] - rcmFMMP[2]*vcmFMMP[0]);
      lFMMP[2] = (rcmFMMP[0]*vcmFMMP[1] - rcmFMMP[1]*vcmFMMP[0]);
      
      time = i*5.0/200;
      r = sqrt(rcmFMMP[0]*rcmFMMP[0]+rcmFMMP[1]*rcmFMMP[1]+rcmFMMP[2]*rcmFMMP[2]);
      v = sqrt(vcmFMMP[0]*vcmFMMP[0]+vcmFMMP[1]*vcmFMMP[1]+vcmFMMP[2]*vcmFMMP[2]);
      l = sqrt(lFMMP[0]*lFMMP[0]+lFMMP[1]*lFMMP[1]+lFMMP[2]*lFMMP[2]);
      
      fprintf(posFMMP,"%lf %e %e %e %e %e %e %e %e %e %e %e %e\n",
	      time,
	      rcmFMMP[0],rcmFMMP[1],rcmFMMP[2],r,
	      vcmFMMP[0],vcmFMMP[1],vcmFMMP[2],v,
	      lFMMP[0],lFMMP[1],lFMMP[2],l); 

      
      free(particles);

    }

  fclose(posSMMP);
  fclose(posTMMP);
  fclose(posFMMP);
  return 0;
}
